/*
 * movement.c
 *
 *  Created on: May 23, 2021
 *      Author: kirk_
 */

#include "movement.h"

void Move(float d)
{
	d /= (float)Movement_Normalize;
	PositionX.Target += d;
}

void MoveForward(uint32_t d)
{
	Move((float)d);
}

void MoveBackward(uint32_t d)
{
	Move(-(float)d);
}

#if Mech_Wheel
void Move(float x, float y)
{
	MoveX(x);
	MoveY(y);
}

void MoveX(float x)
{
	x /= (float)Movement_Normalize;
	PositionX.Target += x;
	PositionY.Target += x;
}

void MoveY(float y)
{
	y /= (float)Movement_Normalize;
	PositionX.Target += y;
	PositionY.Target -= y;
}

void MoveForward(uint32_t d)
{
	MoveX((float)d);
}

void MoveBackward(uint32_t d)
{
	MoveX(-(float)d);
}

void MoveLeft(uint32_t d)
{
	MoveY((float)d);
}

void MoveRight(uint32_t d)
{
	MoveY(-(float)d);
}
#endif
